% **************************************************************************
% 函 数 名：GyroPrecession.m
% 函数功能：陀螺进动效应动画
% 作    者：TMRNic
% 创建时间：2023-10-31
% 其他说明：不是严格的进动仿真程序，仅仅演示进动的效果
% 参考信息：《捷联惯导算法与组合导航原理》（第二版）西工大出版社, 2023.
% 修改信息：无
% **************************************************************************
start;              % 初始化

N = 200;            % 仿真长度
avp=zeros(9,1);     % 初始avp，在此动画中没有用位置速度
sx = 10;  xx = 10;
plan = 50;          % 飞机尺寸，此动画中没有意义
precessionAngle = 60;       % 进动角度
insprieAngle = -20;         % 转动角度
figure;
for i=1:N
    t = mod(i,40);          % 一个周期一分为四
    if t<=5
        avp(1:3) = [precessionAngle*sin(t*2*pi/20).*glv.deg;0;insprieAngle*sin(t*2*pi/20).*glv.deg];
    elseif t>10 && t<=15
        avp(1:3) = [precessionAngle*sin((t-5)*2*pi/20).*glv.deg;0;insprieAngle*sin((t-5)*2*pi/20).*glv.deg];
    elseif t>20 && t<=25
        avp(1:3) = [precessionAngle*sin((t-10)*2*pi/20).*glv.deg;0;insprieAngle*sin((t-10)*2*pi/20).*glv.deg];
    elseif t>30 && t<=35
        avp(1:3) = [precessionAngle*sin((t-15)*2*pi/20).*glv.deg;0;insprieAngle*sin((t-15)*2*pi/20).*glv.deg];
    end  
    hold off;
    plotCylinder3D(avp(1:3),2*plan/3, i);       % 绘制陀螺系统
%     xlabel('东向位置/m');  ylabel('北向位置/m');  zlabel('高度/m');
    axis([xx-3*plan sx+3*plan xx-3*plan sx+3*plan xx-3*plan sx+3*plan]); grid on    % 绘制坐标系统
    drawnow
end

